#include <MathVector.hpp>

#ifndef _CLASSMATHINGMATRIX_
#define _CLASSMATHINGMATRIX_

namespace Mathing {

/**
 * This is a generic class that represents an arithmetic matrix. The elements type is defined by
 * the template argument T, its order is defined by MxN, for square matrices the order is
 * defined by MxM and there is an especialization of a 1x1 matrix wich is used to stop the
 * determinant() method recursivity.
 *
 * @author Marcelo R. Villarreal Fasanelli
 */
template<class T, unsigned int M, unsigned int N>
class Matrix
{
public:
	/**
	 * Destructor: Releases the memory allocated by this object.
	 */
	~Matrix() {}

	/**
	 * Creates a new matrix. It sets all of its elements to zero.
	 */
	Matrix() {}

	/**
	 * Access operator: the access is in both directions, the element can be read and written.
	 * @param index Index of the acceded element.
	 * @throws IndexOutOfBoundsException When index is greater than matrix bounds.
	 */
	MathVector<T, N>& operator[](unsigned int index) {
		if (M <= index) {
			throw Exceptions::IndexOutOfBoundsException("It is tring to get an element out of matrix bounds");
		}
		return this->mat[index];
	}

	/**
	 * Returns the number of rows.
	 * @return The number of rows.
	 */
	unsigned int getM() {
		return M;
	}

	/**
	 * Returns the number of columns.
	 * @return The number of columns.
	 */
	unsigned int getN() {
		return N;
	}

	/**
	 * This method returns the transpose of a matrix, the rows and columns of
	 * each element are interchanged but their values.
	 * @return A matrix with the new values.
	 */
	Matrix<T, N, M> transpose() {
		Matrix<T, N, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int n=0 ; n<N ; n++)
				mat[n][m] = (*this)[m][n];
		return mat;
	}

	/**
	 * Sets all matrix elements to its oposite sign.
	 * @return A matrix with all its elements reverse.
	 */
	Matrix<T, M, N> operator-() {
		Matrix<T, M, N> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = -(*this)[m];
		return mat;
	}

	/**
	 * Product with single value, each matrix element is multiplied by
	 * cohef.
	 * @param cohef A single value to multiplicate the matrix.
	 * @return A matrix with the new values.
	 */
	Matrix<T, M, N> operator*(T cohef) {
		Matrix<T, M, N> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = cohef * (*this)[m];
		return mat;
	}

	/**
	 * Product with a single value.
	 * @param cohef A single value to multiplicate the matrix.
	 * @return A matrix with the new values.
	 */
	Matrix<T, M, N> operator*=(T cohef) {
		return (*this) = (*this)*cohef;
	}

	/**
	 * Cocient betwen a matrix and a value, this operation can be done
	 * in one direction (matrix=dividend, cohef=divisor). Each matrix
	 * element are afected.
	 * @param cohef A single value to do the cocient with.
	 * @return A matrix with the new values.
	 * @throws ArithmeticException If cohef == 0
	 */
	Matrix<T, M, N> operator/(T cohef) throw (ArithmeticException) {
		Matrix<T, M, N> mat;
		try {
			for (unsigned int m=0 ; m<M ; m++)
				mat[m] = (*this)[m]/cohef;
		} catch (Exceptions::ArithmeticException& e) {
			throw Exceptions::ArithmeticException("A matrix can't be divided by zero.", e);
		}
		return mat;
	}

	/**
	 * Cocient and assignation, first it is done the cocient then the result
	 * is assigned.
	 * @param cohef Value to do the cocient with.
	 * @return The matrix with the new values.
	 */
	Matrix<T, M, N> operator/=(T cohef) throw (ArithmeticException) {
		return (*this) = (*this)/cohef;
	}

	/**
	 * Adition: It adds two matrices and returns the result.
	 * @param matrix The matrix to be added.
	 * @return A new resulting matrix.
	 */
	Matrix<T, M, N> operator+(Matrix<T, M, N> matrix) {
		Matrix<T, M, N> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = (*this)[m] +  matrix[m];
		return mat;
	}

	/**
	 * Substraction: It substracts two matrices and returns the result.
	 * @param matrix The matrix to be substracted.
	 * @return A new resulting matrix.
	 */
	Matrix<T, M, N> operator-(Matrix<T, M, N> matrix) {
		Matrix<T, M, N> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = (*this)[m] - matrix[m];
		return mat;
	}

	template<unsigned int R> Matrix<T, M, R> operator*(Matrix<T, N, R> matrix) {
		Matrix<T, M, R> mat;
		Matrix<T, R, N> transposeMat = matrix.transpose();

		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int r=0 ; r<R ; r++)
				mat[m][r] = (*this)[m] % transposeMat[r];
		return mat;
	}

	Matrix<T, M, N> operator+=(Matrix<T, M, N> matrix) {
		return (*this) = (*this) + matrix;
	}

	Matrix<T, M, N> operator-=(Matrix<T, M, N> matrix) {
		return (*this) = (*this) - matrix;
	}

	MathVector<T, N> operator*(MathVector<T, N> V) {
		MathVector<T, N> vec;
		Matrix<T, M, N> mat = this->transpose();
		for (unsigned int n=0 ; n<N ; n++)
			vec[n] = mat[n]%V;
		return vec;
	}

	bool operator==(Matrix<T, M, N> matrix) {
		bool eq = true;
		for (unsigned int m=0 ; (m<M) && (eq) ; m++)
			eq &= (*this)[m] == matrix[m];
		return eq;
	}

	bool operator!=(Matrix<T, M, N> matrix) {
		return !((*this)==matrix);
	}

	void printToStdOut() {
		for (unsigned int m=0 ; m<M ; m++)
			this->mat[m].printToStdOut();
		std::cout << std::endl;
	}

private:
	Mathing::MathVector<T, N> mat[M];
};

template<class T, unsigned int M, unsigned int N>
Matrix<T, M, N> operator*(T cohef, Matrix<T, M, N> mat) 
{
	return mat * cohef;
}



/**
 * Square matrix specialization.
 */
template<class T, unsigned int M>
class Matrix<T, M, M>
{
public:
	~Matrix() {}
	Matrix() {}

	MathVector<T, M>& operator[](unsigned int index) {
		if (M <= index) {
			throw Exceptions::IndexOutOfBoundsException("It is trying to get an element out of matrix bounds");
		}
		return this->mat[index];
	}

	unsigned int getM() {
		return M;
	}

	unsigned int getN() {
		return M;
	}

	T determinant() {
		T det = 0;
		Matrix<T, M, M>& mat = *this;
		for (unsigned int n=0 ; n<M ; n++)
			det += mat[0][n] * mat.adjoint(0, n);
		return det;
	}

	Matrix<T, M, M> transpose() {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int n=0 ; n<M ; n++)
				mat[n][m] = (*this)[m][n];
		return mat;
	}

	Matrix<T, M, M> adjoint() {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int n=0 ; n<M ; n++)
				mat[m][n] += this->adjoint(m, n);
		return mat;
	}

	T adjoint(unsigned int i, unsigned int j) {
		Matrix<T, M-1, M-1> mat;
		T* mt = (T*)&mat;
		unsigned int u = 0;

		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int n=0 ; n<M ; n++)
				if ( (m != i) && (n != j) )
					mt[u++] = (*this)[m][n];
		return mat.determinant() * (((i+j)%2)==0 ? 1 : -1);
	}

	Matrix<T, M, M> inverse() throw (ArithmeticException) {
		Matrix<T, M, M> mat;
		try {
			mat = this->adjoint().transpose() / this->determinant();
		} catch (Exceptions::ArithmeticException& e) {
			throw Exceptions::ArithmeticException("The inverse of a matrix with a zero determinant does not exist.", e);
		}
		return mat;
	}

	Matrix<T, M, M> operator-() {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = -(*this)[m];
		return mat;
	}

	Matrix<T, M, M> operator*(T cohef) {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = cohef * (*this)[m];
		return mat;
	}

	Matrix<T, M, M> operator*=(T cohef) {
		return (*this) = (*this)*cohef;
	}

	Matrix<T, M, M> operator/(T cohef) throw (ArithmeticException) {
		Matrix<T, M, M> mat;
		try {
			for (unsigned int m=0 ; m<M ; m++)
				mat[m] = (*this)[m]/cohef;
		} catch (Exceptions::ArithmeticException& e) {
			throw Exceptions::ArithmeticException("A matrix can't be divided by zero.", e);
		}
		return mat;
	}

	Matrix<T, M, M> operator/=(T cohef) throw (ArithmeticException) {
		return (*this) = (*this)/cohef;
	}

	Matrix<T, M, M> operator+(Matrix<T, M, M> matrix) {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = (*this)[m] +  matrix[m];
		return mat;
	}

	Matrix<T, M, M> operator-(Matrix<T, M, M> matrix) {
		Matrix<T, M, M> mat;
		for (unsigned int m=0 ; m<M ; m++)
			mat[m] = (*this)[m] - matrix[m];
		return mat;
	}

	template<unsigned int R> Matrix<T, M, R> operator*(Matrix<T, M, R> matrix) {
		Matrix<T, M, R> mat;
		Matrix<T, R, M> transposeMat = matrix.transpose();

		for (unsigned int m=0 ; m<M ; m++)
			for (unsigned int r=0 ; r<R ; r++)
				mat[m][r] = (*this)[m] % transposeMat[r];
		return mat;
	}

	Matrix<T, M, M> operator/(Matrix<T, M, M> matrix) throw (ArithmeticException) {
		Matrix<T, M, M> mat;
		try {
			mat = (*this) * matrix.inverse();
		} catch (Exceptions::ArithmeticException& e) {
			throw Exceptions::ArithmeticException("The dividend matrix cannot be inverted.", e);
		}
		return mat;
	}

	Matrix<T, M, M> operator+=(Matrix<T, M, M> matrix) {
		return (*this) = (*this) + matrix;
	}

	Matrix<T, M, M> operator-=(Matrix<T, M, M> matrix) {
		return (*this) = (*this) - matrix;
	}

	Matrix<T, M, M> operator*=(Matrix<T, M, M> matrix) {
		return (*this) = (*this) * matrix;
	}

	Matrix<T, M, M> operator/=(Matrix<T, M, M> matrix) throw (ArithmeticException) {
		return (*this) = (*this) / matrix;
	}

	MathVector<T, M> operator*(MathVector<T, M> V) {
		MathVector<T, M> vec;
		Matrix<T, M, M> mat = this->transpose();
		for (unsigned int n=0 ; n<M ; n++)
			vec[n] = mat[n]%V;
		return vec;
	}

	bool operator==(Matrix<T, M, M> matrix) {
		bool eq = true;
		for (unsigned int m=0 ; (m<M) && (eq) ; m++)
			eq &= (*this)[m] == matrix[m];
		return eq;
	}

	bool operator!=(Matrix<T, M, M> matrix) {
		return !((*this)==matrix);
	}

	void printToStdOut() {
		for (unsigned int m=0 ; m<M ; m++)
			this->mat[m].printToStdOut();
		std::cout << std::endl;
	}
	
	static Matrix<T, M, M> IDENTITY;

private:
	Mathing::MathVector<T, M> mat[M];

	static Matrix<T, M, M> getIdentity();
};

template<class T, unsigned int M>
Matrix<T, M, M> Matrix<T, M, M>::IDENTITY = Matrix<T, M, M>::getIdentity();

template<class T, unsigned int M>
Matrix<T, M, M> Matrix<T, M, M>::getIdentity()
{
	Matrix<T, M, M> mat;
	for (unsigned int m=0 ; m<M ; m++)
		for (unsigned int n=0 ; n<M ; n++)
			mat[m][n] = (n==m ? 1 : 0);
	return mat;
}

template<class T, unsigned int M>
Matrix<T, M, M> operator*(T cohef, Matrix<T, M, M> mat) 
{
	return mat * cohef;
}


template<class T>
class Matrix<T, 1, 1>
{
public:
	~Matrix() {}
	Matrix() {}

	MathVector<T, 1>& operator[](unsigned int index);

	T determinant();
	Matrix<T, 1, 1> transpose();
	Matrix<T, 1, 1> inverse() throw (ArithmeticException);

	Matrix<T, 1, 1> operator-() {
		Matrix<T, 1, 1> mat;
		mat[0] = -(*this)[0];
		return mat;
	}

	Matrix<T, 1, 1> operator*(T cohef);
	Matrix<T, 1, 1> operator*=(T cohef);
	Matrix<T, 1, 1> operator/(T cohef) throw (ArithmeticException);
	Matrix<T, 1, 1> operator/=(T cohef) throw (ArithmeticException);

	Matrix<T, 1, 1> operator+(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator-(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator*(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator/(Matrix<T, 1, 1> matrix) throw (ArithmeticException);
	Matrix<T, 1, 1> operator+=(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator-=(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator*=(Matrix<T, 1, 1> matrix);
	Matrix<T, 1, 1> operator/=(Matrix<T, 1, 1> matrix) throw (ArithmeticException);

	MathVector<T, 1> operator*(MathVector<T, 1> V);

	bool operator==(Matrix<T, 1, 1> matrix);
	bool operator!=(Matrix<T, 1, 1> matrix);

	static Matrix<T, 1, 1> IDENTITY;

	void printToStdOut();

private:
	Mathing::MathVector<T, 1> mat[1];
};

template<class T>
T Mathing::Matrix<T, 1, 1>::determinant()
{
	return this->mat[0][0];
}


typedef Matrix<char,   4, 4> Matrix4c;
typedef Matrix<short,  4, 4> Matrix4s;
typedef Matrix<int,    4, 4> Matrix4i;
typedef Matrix<long,   4, 4> Matrix4l;
typedef Matrix<float,  4, 4> Matrix4f;
typedef Matrix<double, 4, 4> Matrix4d;
typedef Matrix<unsigned char,  4, 4> Matrix4uc;
typedef Matrix<unsigned short, 4, 4> Matrix4us;
typedef Matrix<unsigned int,   4, 4> Matrix4ui;
typedef Matrix<unsigned long,  4, 4> Matrix4ul;

}// namespace Mathing

#endif
